#include <iostream>
#include <sstream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

struct PointComparator {
    std::vector<cv::Point> pts;
    PointComparator(const std::vector<cv::Point>& _pts) {
        pts = _pts;
    }
    bool operator()(int idx1, int idx2) const {
        cv::Point p1 = pts[idx1];
        cv::Point p2 = pts[idx2];
        if (p1.x < p2.x) {
            return true;
        }
        if (p1.x == p2.x && p1.y < p2.y) {
            return true;
        }
        return false;
    }
};

static int _jack_sklansky(const std::vector<cv::Point>& pts, const std::vector<int>& indices, int start, int end, std::vector<int>& stack, int nsign, int sign2) {
    int increaseStep = (end > start ? 1 : -1);
    int prevIndex = start, curIndex = prevIndex + increaseStep, nxtIndex = curIndex + increaseStep;
    if (start == end || (pts[indices[start]].x == pts[indices[end]].x && pts[indices[start]].y == pts[indices[end]].y)) {
        stack.push_back(0);
        return 1;
    }
    
    stack.resize(pts.size()+2);
    stack[0] = prevIndex;
    stack[1] = curIndex;
    stack[2] = nxtIndex;
    int stackSize = 3;
    
    while (nxtIndex != end + increaseStep) {
        int curY = pts[indices[curIndex]].y;
        int nxtY = pts[indices[nxtIndex]].y;
        int bY = nxtY - curY;
        
        int signBY = (bY > 0 ? 1 : (bY < 0 ? -1 : 0));
        if (signBY != nsign) {
            int aX = pts[indices[curIndex]].x - pts[indices[prevIndex]].x;
            int bX = pts[indices[nxtIndex]].x - pts[indices[curIndex]].x;
            int aY = curY - pts[indices[prevIndex]].y;
            
            int convexity = aY*bX - aX*bY;
            int signConvexity = (convexity > 0 ? 1 : (convexity < 0 ? -1 : 0));
            if (signConvexity == sign2 && (aX != 0 || aY != 0)) {
                prevIndex = curIndex;
                curIndex = nxtIndex;
                nxtIndex += increaseStep;
                stack[stackSize] = nxtIndex;
                stackSize++;
            } else {
                if (prevIndex == start) {
                    curIndex = nxtIndex;
                    stack[1] = curIndex;
                    nxtIndex += increaseStep;
                    stack[2] = nxtIndex;
                } else {
                    stack[stackSize-2] = nxtIndex;
                    curIndex = prevIndex;
                    prevIndex = stack[stackSize-4];
                    stackSize--;
                }
            }
        } else {
            nxtIndex += increaseStep;
            stack[stackSize-1] = nxtIndex;
        }
    }
    stackSize--;
    stack.resize(stackSize);
    return stackSize;
}

static void _convex_hull(const std::vector<cv::Point>& pts, std::vector<int>& hull, bool closewise) {
    int total = pts.size();
    hull.clear();
    if (total == 0) {
        return;
    }
    std::vector<int> indices(total);
    for (int i = 0; i < total; i++) {
        indices[i] = i;
    }
    int minYIndex = 0, maxYIndex = 0;
    std::sort(indices.begin(), indices.end(), PointComparator(pts));
    for (int i = 1; i < total; i++) {
        int y = pts[indices[i]].y;
        if (pts[indices[minYIndex]].y > y) {
            minYIndex = i;
        }
        if (pts[indices[maxYIndex]].y < y) {
            maxYIndex = i;
        }
    }
    
    if (pts[0].x == pts[total-1].x && pts[0].y == pts[total-1].y) {
        hull.push_back(0);
    } else {
        std::vector<int> topLeftStack, topRightStack;
        int topLeftCount = _jack_sklansky(pts, indices, 0, maxYIndex, topLeftStack, -1, 1);
        int topRightCount = _jack_sklansky(pts, indices, total-1, maxYIndex, topRightStack, -1, -1);
        if (!closewise) {
            std::vector<int> tmp;
            tmp.resize(topLeftCount);
            std::copy(topLeftStack.begin(), topLeftStack.end(), tmp.begin());
            topLeftStack.resize(topRightCount);
            std::copy(topRightStack.begin(), topRightStack.end(), topLeftStack.begin());
            topRightStack.resize(topLeftCount);
            std::copy(tmp.begin(), tmp.end(), topRightStack.begin());
            std::swap(topLeftCount, topRightCount);
        }
        for (int i = 0; i < topLeftCount-1; i++) {
            hull.push_back(indices[topLeftStack[i]]);
        }
        for (int i = topRightCount-1; i > 0; i--) {
            hull.push_back(indices[topRightStack[i]]);
        }
        
        std::vector<int> bottomLeftStack, bottomRightStack;
        int bottomLeftCount = _jack_sklansky(pts, indices, 0, minYIndex, bottomLeftStack, 1, -1);
        int bottomRightCount = _jack_sklansky(pts, indices, total-1, minYIndex, bottomRightStack, 1, 1);
        if (closewise) {
            std::vector<int> tmp;
            tmp.resize(bottomLeftCount);
            std::copy(bottomLeftStack.begin(), bottomLeftStack.end(), tmp.begin());
            bottomLeftStack.resize(bottomRightCount);
            std::copy(bottomRightStack.begin(), bottomRightStack.end(), bottomLeftStack.begin());
            bottomRightStack.resize(bottomLeftCount);
            std::copy(tmp.begin(), tmp.end(), bottomRightStack.begin());
            std::swap(bottomLeftCount, bottomRightCount);
        }
        for (int i = 0; i < bottomLeftCount-1; i++) {
            hull.push_back(indices[bottomLeftStack[i]]);
        }
        for (int i = bottomRightCount-1; i > 0; i--) {
            hull.push_back(indices[bottomRightStack[i]]);
        }
    }
}

static bool _is_contour_convex(const std::vector<cv::Point>& pts) {
    int total = pts.size();
    if (total < 3) {
        return false;
    }
    
    cv::Point prev = pts[total-2];
    cv::Point cur = pts[total-1];
    cv::Point nxt;
    int direction = 0;
    for (int i = 0; i < total; i++) {
        nxt = pts[i];
        int aX = cur.x - prev.x;
        int aY = cur.y - prev.y;
        int bX = nxt.x - cur.x;
        int bY = nxt.y - cur.y;
        int direct = aY*bX-aX*bY;
        if (direct == 0) {
            prev = cur;
            cur = nxt;
            continue;
        }
        if (direction == 0) {
            direction = direct;
        } else {
            if ((direct < 0 && direction > 0) || (direct > 0 && direction < 0)) {
                return false;
            }
        }
        prev = cur;
        cur = nxt;
    }
    return true;
}

static void _convexity_defects(const std::vector<cv::Point>& pts, const std::vector<int>& hull, std::vector<cv::Vec4i>& defects) {
    int npoints = pts.size();
    defects.clear();
    if (npoints <= 3) {
        return;
    }
    int hpoints = hull.size();
    if (hpoints < 3) {
        return;
    }
    
    bool is_different_direction = ((hull[1] > hull[0]) + (hull[2] > hull[1]) + (hull[0] > hull[2])) != 2;
    int hull_cur = hull[hpoints-1];
    if (is_different_direction) {
        hull_cur = hull[0];
    }
    
    for (int i = 0; i < hpoints; i++) {
        int hull_nxt = hull[i];
        if (is_different_direction) {
            hull_nxt = hull[hpoints-1-i];
        }
        
        cv::Point pt_cur = pts[hull_cur], pt_nxt = pts[hull_nxt];
        double dx0 = pt_nxt.x - pt_cur.x;
        double dy0 = pt_nxt.y - pt_cur.y;
        double scale = 0;
        if (!(dx0 == 0 && dy0 == 0)) {
            scale = 1. / std::sqrt(dx0*dx0+dy0*dy0);
        }
        
        int defect_deepest_point = -1;
        double defect_distance = 0;
        bool is_defect = false;
        int j = hull_cur;
        while (true) {
            j++;
            if (j >= npoints) {
                j = 0;
            }
            if (j == hull_nxt) {
                break;
            }
            
            double dx = pts[j].x - pt_cur.x;
            double dy = pts[j].y - pt_cur.y;
            double distance = std::fabs(-dy0*dx+dx0*dy)*scale;
            if (distance > defect_distance) {
                defect_distance = distance;
                defect_deepest_point = j;
                is_defect = true;
            }
        }
        if (is_defect) {
            int depth = cvRound(defect_distance*256);
            defects.push_back(cv::Vec4i(hull_cur, hull_nxt, defect_deepest_point, depth));
        }
        hull_cur = hull_nxt;
    }
}

int main(int argc, char **argv) {
    /*
    cv::RNG rng(200);
    cv::Mat src = cv::Mat::zeros(500, 500, CV_8UC3);
    std::vector<cv::Point> pts;
    for (int i = 0; i < 100; i++) {
        cv::Scalar x = cv::Scalar::all(rng.uniform(50, 450));
        cv::Scalar y = cv::Scalar::all(rng.uniform(50, 450));
        pts.push_back(cv::Point(x[0], y[0]));
        cv::circle(src, pts[i], 2, cv::Scalar(255, 0, 255), cv::FILLED);
    }
    //cv::Mat src2 = src.clone();
    
    std::vector<cv::Point> hull;
    bool closewise = false;
    bool returnPoints = true;
    cv::convexHull(pts, hull, closewise, returnPoints);
    hull.push_back(pts[rng.uniform(0, pts.size()-1)]);
    for (int i = 0; i < hull.size(); i++) {
        cv::line(src, hull[i], hull[(i+1)%hull.size()], cv::Scalar(0, 0, 255), 1);
    }
    
    //std::vector<int> hullIndices;
    //_convex_hull(pts, hullIndices, closewise);
    //for (int i = 0; i < hullIndices.size(); i++) {
    //    cv::line(src2, pts[hullIndices[i]], pts[hullIndices[(i+1)%hullIndices.size()]], cv::Scalar(255, 255, 0), 1);
    //}
    
    bool isContourConvex = cv::isContourConvex(hull);
    std::cout << "isContourConvex = " << isContourConvex << std::endl;
    bool is_contour_convex = _is_contour_convex(hull);
    std::cout << "is_contour_convex = " << is_contour_convex << std::endl;
    
    cv::imshow("src", src);
    //cv::imshow("src2", src2);
    cv::waitKey(0);
    */
    
    cv::Mat src = cv::Mat::zeros(500, 500, CV_8UC3);
    std::vector<cv::Point> pts;
    pts.push_back(cv::Point(225, 50));
    pts.push_back(cv::Point(225, 200));
    pts.push_back(cv::Point(100, 300));
    pts.push_back(cv::Point(225, 275));
    pts.push_back(cv::Point(250, 450));
    pts.push_back(cv::Point(275, 275));
    pts.push_back(cv::Point(400, 300));
    pts.push_back(cv::Point(275, 200));
    pts.push_back(cv::Point(275, 50));
    for (int i = 0; i < pts.size(); i++) {
        cv::circle(src, pts[i], 2, cv::Scalar(255, 0, 255), cv::FILLED);
        cv::line(src, pts[i], pts[(i+1)%pts.size()], cv::Scalar(255, 0, 255), 1);
    }
    
    std::vector<int> hull;
    bool closewise = false;
    bool returnPoints = false;
    cv::convexHull(pts, hull, closewise, returnPoints);
    for (int i = 0; i < hull.size(); i++) {
        cv::line(src, pts[hull[i]], pts[hull[(i+1)%hull.size()]], cv::Scalar(0, 0, 255), 1);
    }
    
    cv::Mat tmp = src.clone();
    std::vector<cv::Mat> defects;
    std::vector<cv::Vec4i> convexityDefects;
    cv::convexityDefects(pts, hull, convexityDefects);
    for (int i = 0; i < convexityDefects.size(); i++) {
        cv::Mat mat = tmp.clone();
        cv::Vec4i defect = convexityDefects[i];
        cv::line(mat, pts[defect[0]], pts[defect[1]], cv::Scalar(255, 0, 0), 1);
        cv::line(mat, pts[defect[0]], pts[defect[2]], cv::Scalar(255, 0, 0), 1);
        cv::line(mat, pts[defect[2]], pts[defect[1]], cv::Scalar(255, 0, 0), 1);
        std::cout << "distance of defect " << i << " = " << (defect[3]&0x00ff) << "/256.0 = " << ((defect[3]&0x00ff)/256.0) << std::endl;
        defects.push_back(mat);
    }
    
    std::vector<cv::Vec4i> convexityDefects1;
    _convexity_defects(pts, hull, convexityDefects1);
    bool result_is_same = true;
    if (convexityDefects.size() != convexityDefects1.size()) {
        result_is_same = false;
    }
    if (result_is_same) {
        for (int i = 0; i < convexityDefects.size(); i++) {
            cv::Vec4i defect1 = convexityDefects[i];
            cv::Vec4i defect2 = convexityDefects1[i];
            if (defect1[0] != defect2[0] || defect1[1] != defect2[1] || defect1[2] != defect2[2] || defect1[3] != defect2[3]) {
                result_is_same = false;
                break;
            }
        }
    }
    std::cout << (result_is_same ? "Two results are the same!" : "Two results are not the same!") << std::endl;
    
    cv::imshow("src", src);
    for (int i = 0; i < defects.size(); i++) {
        cv::imshow(std::to_string(i), defects[i]);
    }
    cv::waitKey(0);
    return 0;
}
